#!/usr/bin/env python
#coding=utf-8

import rospy
import string
import sys

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
from matplotlib.pyplot import MultipleLocator

from ant_msgs.msg import ControlCmd2
#from ant_msgs.msg import VehicleState

#此速度文件为三车仿真实验数据
df_news = pd.read_table('/home/seu/projects/littleAnt_ws_zsx/src/data_record/scripts/speed.txt',header = None)
v_lead = df_news[1]
#v_ego = df_news[1]
#v_later = df_news[2]

i = 0

#实现静态变量功能
def f():
	if not hasattr(f, 'x'):
		f.x = 0
	f.x+=1
	return f.x

def LittleAntCmdTopicCallback(RealSpeedCmd):
	global i
#	print(i)
#	NewSpeedPub.driverless = RealSpeedCmd.driverless
	NewSpeedPub.set_gear = RealSpeedCmd.set_gear
	#速度替换
	if i <= (len(v_lead) - 1):
		NewSpeedPub.set_speed = v_lead[i]*3.6
	else:
		NewSpeedPub.set_speed = RealSpeedCmd.set_speed
	NewSpeedPub.set_brake = RealSpeedCmd.set_brake
	NewSpeedPub.set_accelerate = RealSpeedCmd.set_accelerate
	NewSpeedPub.set_roadWheelAngle = RealSpeedCmd.set_roadWheelAngle
	NewSpeedPub.set_emergencyBrake = RealSpeedCmd.set_emergencyBrake
#	NewSpeedPub.turnlight_r = RealSpeedCmd.turnlight_r
#	NewSpeedPub.turnlight_l = RealSpeedCmd.turnlight_l
#	NewSpeedPub.low_beam = RealSpeedCmd.low_beam
#	NewSpeedPub.high_beam = RealSpeedCmd.high_beam
#	NewSpeedPub.brake_light = RealSpeedCmd.brake_light
#	NewSpeedPub.horn = RealSpeedCmd.horn
	PubSimSpeedCmd.publish(NewSpeedPub)
	i = f()

if __name__ == '__main__':
	rospy.init_node('speed_pub_node')
	RealSpeedCmd = ControlCmd2()
	NewSpeedPub = ControlCmd2()
	#订阅小蚂蚁路径跟踪生成的消息指令
	LittleAntCmdTopic = rospy.get_param('little_ant_cmd', '/ControlCmd2')
	rospy.Subscriber(LittleAntCmdTopic, ControlCmd2, LittleAntCmdTopicCallback, queue_size=1)
	#订阅路径跟踪消息后,重新生成指令
	SpeedCmdTopic = rospy.get_param('sim__speed_cmd', '/SimSpeedCmd')
	PubSimSpeedCmd = rospy.Publisher(SpeedCmdTopic,ControlCmd2, queue_size=10)
	rospy.spin()
